#include "camera_perception.h"
#include "rcutils/filesystem.h"
#include <opencv2/opencv.hpp>
#include <unistd.h>
namespace camera_perception {

static std::vector<Box> mockBox(void)
{
    static std::vector<Box> boxs {
        { -0.164593, 6.52935, -1.65531, 4.23076, 1.70605, 1.43383, 1.59898, 0, 0, 0.837354, 0 },
        { -7.50769, -8.58763, -1.83721, 4.28482, 1.78443, 1.52519, 0.118129, 0, 0, 0.828384, 0 },
        { 5.19405, -11.7293, -2.12557, 4.48607, 1.84648, 1.59644, -1.5411, 0, 0, 0.804866, 0 },
        // { -1.76907, -5.49038, -1.89324, 4.33533, 1.78443, 1.60308, 1.2552, 0, 0, 0.798187, 0 },
        // { -5.198, -25.1756, -2.34345, 4.42948, 1.88382, 1.70023, 3.18485, 0, 0, 0.723016, 0 },
        // { 9.14442, 15.5496, -1.44631, 0.738435, 0.652781, 1.75334, 2.84265, 0, 0, 0.722821, 8 },
        // { -16.2552, -8.32846, -1.52663, 4.68303, 1.93888, 1.55186, 3.07619, 0, 0, 0.71721, 0 },
        // { -20.4232, -18.729, -1.76425, 4.53452, 1.90696, 1.53041, 0.0802503, 0, 0, 0.698741, 0 },
        // { -16.6841, -11.3802, -1.60015, 4.55671, 1.91442, 1.72363, 3.23279, 0, 0, 0.627609, 0 },
        // { -20.3354, -5.7867, -1.21663, 4.69219, 1.93415, 1.58324, -0.0290354, 0, 0, 0.616474, 0 },
        // { -25.333, -21.2659, -0.956916, 0.588731, 0.637659, 1.77835, 0.270523, 0, 0, 0.548915, 8 },
        // { -10.7247, -24.536, -2.19859, 4.78006, 2.02104, 1.85099, 3.18553, 0, 0, 0.513005, 0 },
        // { -5.34511, -27.7657, -2.4333, 4.39072, 1.90509, 1.71356, 3.11775, 0, 0, 0.491898, 0 },
        // { -10.275, -36.4527, -2.49934, 4.48169, 1.93793, 1.62039, 3.15031, 0, 0, 0.471223, 0 },
        // { -25.9758, -21.9291, -0.867176, 0.576781, 0.637037, 1.78967, 0.353824, 0, 0, 0.447641, 8 },
        // { 8.62853, 14.838, -1.43294, 0.731437, 0.652462, 1.76709, 2.96136, 0, 0, 0.437643, 8 },
        // { -21.3102, -34.828, -2.13761, 4.71976, 2.0014, 1.76365, -0.00540733, 0, 0, 0.430207, 0 },
        // { -10.1973, -31.6817, -2.39704, 4.41221, 1.94457, 1.63429, 3.14254, 0, 0, 0.42477, 0 },
        // { -21.2955, -36.4576, -2.17807, 4.64659, 1.9936, 1.71691, -0.0031637, 0, 0, 0.407274, 0 },
        // { -2.85527, -36.4714, -2.68944, 1.95886, 0.705997, 1.54317, -1.53834, 0, 0, 0.39905, 6 },
        // { -0.413752, 14.7203, -1.45821, 4.48169, 1.83749, 1.72279, 1.47614, 0, 0, 0.390127, 0 },
        // { -2.95182, -32.5196, -2.57897, 1.99652, 0.688465, 1.46676, -1.48149, 0, 0, 0.370909, 6 },
        // { -10.2319, -28.5264, -2.3167, 4.45551, 1.96078, 1.68864, 3.17037, 0, 0, 0.369088, 0 },
        // { -21.3193, -33.3131, -2.08987, 4.65113, 1.97905, 1.70438, -0.00456774, 0, 0, 0.359701, 0 },
        // { -10.2516, -34.9323, -2.46914, 4.47731, 1.93793, 1.60856, 3.15508, 0, 0, 0.350755, 0 },
        // { -25.1061, -20.7459, -0.942144, 0.590459, 0.632079, 1.76106, 0.18878, 0, 0, 0.343672, 8 },
        // { 9.73304, 16.242, -1.44521, 0.74988, 0.655336, 1.73207, 2.81513, 0, 0, 0.338952, 8 },
        // { -0.246469, 21.0934, -1.17372, 4.25979, 1.78095, 1.68453, 1.55362, 0, 0, 0.337204, 0 },
        // { -10.2326, -33.2685, -2.42615, 4.42515, 1.93321, 1.60347, 3.14318, 0, 0, 0.310276, 0 },
        // { -21.9411, -28.4738, -1.90819, 4.47731, 1.91816, 1.60151, -0.0204909, 0, 0, 0.30013, 0 },
        // { -2.85156, -34.9247, -2.64857, 2.0171, 0.712403, 1.51887, -1.50805, 0, 0, 0.299412, 6 },
        // { 1.19888, -49.2859, -2.90971, 4.4036, 1.88198, 1.68288, 1.59738, 0, 0, 0.291082, 0 },
        // { -25.9824, -4.37949, 0.0565666, 0.689811, 0.655016, 1.62514, 3.72091, 0, 0, 0.288669, 8 },
        // { -21.254, -38.0368, -2.22118, 4.61944, 1.99457, 1.66653, -1.38283e-05, 0, 0, 0.28587, 0 },
        // { -21.3384, -26.862, -1.895, 4.56116, 1.93699, 1.62912, -0.08847, 0, 0, 0.282988, 0 },
        // { -10.6951, -26.0012, -2.2367, 4.64659, 1.99847, 1.76278, 3.16514, 0, 0, 0.270769, 0 },
        // { -25.2809, -4.35977, -0.026072, 0.680112, 0.652781, 1.6121, 4.13314, 0, 0, 0.265119, 8 },
        // { -5.25928, -44.4907, -2.75859, 4.30579, 1.83928, 1.57207, 3.1874, 0, 0, 0.262464, 0 },
        // { -3.23248, 21.9721, -1.11094, 0.276337, 0.274187, 1.01068, 0.537882, 0, 0, 0.261897, 9 },
        // { -3.3517, 22.5771, -1.10663, 0.281513, 0.273919, 1.04072, 0.582665, 0, 0, 0.257578, 9 },
        // { -5.24669, -46.0723, -2.79487, 4.36506, 1.86551, 1.59644, 3.18932, 0, 0, 0.255529, 0 },
        // { -10.0012, -22.947, -2.15769, 4.75213, 2.00336, 1.8528, 3.16626, 0, 0, 0.249447, 0 },
        // { -22.0072, -30.0407, -1.96521, 4.46422, 1.90789, 1.5763, 0.000185132, 0, 0, 0.24726, 0 },
        // { -3.42648, -32.4987, -2.56391, 2.21751, 0.745681, 1.55413, -1.51548, 0, 0, 0.244363, 6 },
        // { -26.7636, -4.43422, 0.123157, 0.702901, 0.666144, 1.64029, 3.55557, 0, 0, 0.242923, 8 },
        // { -10.1983, -30.0809, -2.36055, 4.47294, 1.96653, 1.67796, 3.16044, 0, 0, 0.238461, 0 },
        // { -21.2721, -39.5704, -2.26996, 4.64659, 2.00336, 1.65437, -0.015316, 0, 0, 0.232484, 0 },
        // { -5.254, -47.6608, -2.8356, 4.3993, 1.88935, 1.62912, 3.18312, 0, 0, 0.2304, 0 },
        // { -21.2931, -31.7565, -2.03941, 4.48607, 1.93132, 1.60112, 0.0130914, 0, 0, 0.22627, 0 },
        // { -5.13614, -37.2902, -2.66677, 4.43814, 1.91723, 1.61092, 3.38049, 0, 0, 0.223377, 0 },
        // { -10.6625, -37.9575, -2.49916, 4.52567, 1.93321, 1.63629, 3.15025, 0, 0, 0.219337, 0 },
        // { -5.26092, -42.9225, -2.72925, 4.289, 1.83032, 1.56594, 3.1766, 0, 0, 0.217338, 0 },
        // { -9.18932, -21.3658, -2.09075, 4.63299, 1.95218, 1.85009, 3.17735, 0, 0, 0.215186, 0 },
        // { -26.5685, -22.1477, -0.795268, 0.583295, 0.634553, 1.80019, 0.501387, 0, 0, 0.205775, 8 },
        // { 6.22112, 12.8288, -1.38934, 0.321497, 0.323387, 0.771421, 3.21123, 0, 0, 0.204819, 9 },
        // { -0.266469, 17.8898, -1.31685, 4.45116, 1.80723, 1.73122, 1.53557, 0, 0, 0.201499, 0 },
        // { -5.29243, -49.2557, -2.87056, 4.45551, 1.90231, 1.66816, 3.16436, 0, 0, 0.200245, 0 },
        // { -27.5682, -4.50756, 0.169608, 0.721153, 0.679448, 1.66328, 3.43617, 0, 0, 0.198685, 8 },
        // { -5.34375, -50.8746, -2.91262, 4.49484, 1.92661, 1.66734, 3.15231, 0, 0, 0.188623, 0 },
        // { -2.83394, -37.9634, -2.685, 1.86733, 0.685279, 1.55793, -1.57055, 0, 0, 0.183447, 6 },
        // { -1.15326, 29.0593, -1.05899, 4.25563, 1.74821, 1.54921, 1.68649, 0, 0, 0.183009, 0 },
        // { -28.3712, -4.55699, 0.196631, 0.741325, 0.694882, 1.6755, 3.30635, 0, 0, 0.172033, 8 },
        // { -5.14866, -38.8634, -2.68056, 4.44247, 1.90138, 1.62951, 3.2937, 0, 0, 0.170923, 0 },
        // { -3.52886, 23.3432, -1.0886, 0.284275, 0.285109, 1.07974, 0.775353, 0, 0, 0.167083, 9 },
        // { -3.50137, -36.4751, -2.65986, 2.53001, 0.879271, 1.55337, -1.55332, 0, 0, 0.167083, 6 },
        // { -5.19778, -41.306, -2.70462, 4.31, 1.84558, 1.58015, 3.19523, 0, 0, 0.165998, 0 },
        // { -24.5643, -4.38042, -0.136824, 0.658704, 0.648017, 1.56365, -1.38249, 0, 0, 0.165189, 8 },
        // { -2.93676, 21.2848, -1.16797, 0.272053, 0.291017, 1.1613, 0.765197, 0, 0, 0.164651, 9 },
        // { -26.7436, -22.6789, -0.835776, 0.581021, 0.635484, 1.81431, 0.658529, 0, 0, 0.164114, 8 },
        // { -2.96902, -30.9911, -2.54258, 1.97422, 0.67713, 1.43173, -1.47594, 0, 0, 0.163312, 6 },
        // { -5.30535, -29.0351, -2.48947, 4.33533, 1.87647, 1.66816, 3.19126, 0, 0, 0.159348, 0 },
        // { -5.18607, -35.7543, -2.64154, 4.37787, 1.90045, 1.59527, 3.57763, 0, 0, 0.153169, 0 },
        // { 1.25553, -46.1801, -2.85768, 4.35655, 1.86825, 1.69195, 1.62008, 0, 0, 0.151153, 0 },
        // { -3.45165, -34.0282, -2.60326, 2.316, 0.784717, 1.56327, -1.52226, 0, 0, 0.145479, 6 },
        // { -29.1509, -4.56107, 0.204895, 0.760946, 0.705308, 1.68864, 3.17729, 0, 0, 0.145358, 8 },
        // { -1.09069, 25.8761, -1.08634, 4.24732, 1.74992, 1.60856, 1.62863, 0, 0, 0.144149, 0 },
        // { -30.7503, -0.320052, 0.100526, 0.752264, 0.675973, 1.7363, 1.92651, 0, 0, 0.142351, 8 },
        // { -1.93493, 49.9165, -0.712166, 4.28064, 1.92661, 1.57015, 3.06066, 0, 0, 0.141875, 0 },
        // { -24.4435, -19.9423, -1.059, 0.598587, 0.626548, 1.7602, 0.180383, 0, 0, 0.141281, 8 },
        // { -3.55923, -29.3032, -2.48878, 2.11287, 0.719043, 1.46247, 4.69928, 0, 0, 0.140101, 6 },
        // { -30.0395, -0.328093, 0.114682, 0.720097, 0.656617, 1.61053, 1.97927, 0, 0, 0.136148, 8 },
        // { -25.7694, -20.7696, -0.807201, 0.585006, 0.634863, 1.72447, 0.166063, 0, 0, 0.134662, 8 },
        // { 0.371531, 23.3998, -1.12048, 4.289, 1.78095, 1.66003, 1.57856, 0, 0, 0.130951, 0 },
        // { -26.012, -5.00437, 0.0303075, 0.70376, 0.649601, 1.55717, 4.16313, 0, 0, 0.128744, 8 },
        // { -3.05024, -29.36, -2.54145, 1.96557, 0.671532, 1.41575, -1.50484, 0, 0, 0.126138, 6 },
        // { -2.41972, -35.7138, -2.70864, 1.9723, 0.708414, 1.47106, -1.50969, 0, 0, 0.124107, 6 },
        // { -2.39567, -33.2687, -2.63801, 2.06191, 0.716765, 1.43104, -1.48184, 0, 0, 0.122313, 6 },
        // { -2.7063, 42.6764, -0.756987, 4.32264, 1.85461, 1.5713, 2.89113, 0, 0, 0.120957, 0 },
        // { 8.38482, 14.2046, -1.44279, 0.744408, 0.652144, 1.7542, 2.96621, 0, 0, 0.120439, 8 },
        // { 0.58001, -51.7347, -3.00081, 4.55671, 1.91255, 1.75078, 1.64795, 0, 0, 0.117167, 0 },
        // { -25.2511, -5.01365, -0.0857518, 0.682274, 0.647069, 1.53603, -1.51557, 0, 0, 0.115961, 8 },
        // { -10.7804, -39.5644, -2.5136, 4.46858, 1.91535, 1.59956, 3.14318, 0, 0, 0.111434, 0 },
        // { -29.9273, -4.54149, 0.203517, 0.783951, 0.718341, 1.70272, 3.05646, 0, 0, 0.108756, 8 },
        // { -31.5086, -0.304654, 0.109212, 0.773306, 0.688465, 1.79667, 1.84177, 0, 0, 0.108378, 8 },
        // { -3.61313, 24.1615, -1.06804, 0.295888, 0.305579, 1.11831, 0.893844, 0, 0, 0.108189, 9 },
        // { -21.2287, -41.1485, -2.32898, 4.65113, 1.99457, 1.64953, -0.0226601, 0, 0, 0.104843, 0 },
        // { -2.43879, -37.2033, -2.73983, 1.88935, 0.704103, 1.49715, -1.53284, 0, 0, 0.102664, 6 },
        // { -3.47143, -30.9516, -2.51813, 2.18847, 0.742774, 1.51554, -1.48423, 0, 0, 0.102125, 6 },
        // { 7.71288, 13.952, -1.42857, 0.676138, 0.625326, 1.66653, 3.0169, 0, 0, 0.101056, 8 },
        // { -21.1925, -25.4016, -1.84751, 4.66477, 1.96749, 1.67305, -0.105101, 0, 0, 0.100702, 0 },

    };
    return boxs;
}

CameraPerception::CameraPerception(void)
    : logger_(rclcpp::get_logger("CameraPerception"))
{
    getInfo();
    getExecutionPath();
    // 打印当前执行路径
    std::string configPath = configPath_ + bevDetConfigurePath;
    std::cout << "Current Path: " << configPath << std::endl;
    if (!fileExists(configPath)) {
        RCLCPP_ERROR(logger_, "Unsupported bevDetConfigurePath:%s", bevDetConfigurePath);
    }
    YAML::Node config = YAML::LoadFile(configPath);
    sample(config);
    std::cout << " modelConfig_ 2" << modelConfig_ << std::endl;
    bevDetPtrs_ = std::make_shared<BEVDet>(modelConfig_, imgN_, sampleData_.param.cams_intrin, sampleData_.param.cams2ego_rot,
        sampleData_.param.cams2ego_trans, imgstageFile_, bevstageFile_);
    if (isMock_) {
        auto pool = mockObjectThread_.startLoop();
        pool->runInLoop([=] {
            mockObjects();
        });
    }
}

CameraPerception::~CameraPerception(void)
{
}

void CameraPerception::mockObjects()
{
    while (1) {
        usleep(100000);
        mockcb_(mockBox());
    }
}

void CameraPerception::getExecutionPath(void)
{
    char currentPath[4096]; // 为路径分配足够的缓冲区
    if (rcutils_get_cwd(currentPath, sizeof(currentPath)) == 0) {
        std::cerr << "Failed to get current working directory." << std::endl;
    }
    configPath_ = std::string(currentPath);
}

void CameraPerception::getInfo(void)
{
    cudaDeviceProp prop;
    int count;
    cudaGetDeviceCount(&count);
    printf("\nGPU has cuda devices: %d\n", count);
    for (int i = 0; i < count; ++i) {
        cudaGetDeviceProperties(&prop, i);
        printf("----device id: %d info----\n", i);
        printf("  GPU : %s \n", prop.name);
        printf("  Capbility: %d.%d\n", prop.major, prop.minor);
        printf("  Global memory: %luMB\n", prop.totalGlobalMem >> 20);
        printf("  Const memory: %luKB\n", prop.totalConstMem >> 10);
        printf("  Shared memory in a block: %luKB\n", prop.sharedMemPerBlock >> 10);
        printf("  warp size: %d\n", prop.warpSize);
        printf("  threads in a block: %d\n", prop.maxThreadsPerBlock);
        printf("  block dim: (%d,%d,%d)\n", prop.maxThreadsDim[0],
            prop.maxThreadsDim[1], prop.maxThreadsDim[2]);
        printf("  grid dim: (%d,%d,%d)\n", prop.maxGridSize[0], prop.maxGridSize[1],
            prop.maxGridSize[2]);
    }
    printf("\n");
}

void CameraPerception::setImages(std::shared_ptr<interface::msg::Images> images)
{
    // if (isMock_) {
    //     std::cout << "setImage" << std::endl;
    //     cb_(mockBox());
    //     std::cout << "setImage" << std::endl;
    //     return;
    // }
    uchar* imgs_dev = nullptr;
    CHECK_CUDA(cudaMalloc((void**)&imgs_dev, imgN_ * 3 * imgW_ * imgH_ * sizeof(uchar)));
    size_t offset = 0;
    size_t index = 0;
    std::cout << "1 :" << images->images.size() << std::endl;
    for (auto& i : images->images) {
        // if (index > 5) {
        //     break;
        // }
        cv::Mat inputMat(i.height, i.width, CV_8UC3, const_cast<unsigned char*>(i.data.data()));
        cv::Mat resizedMat;
        cv::resize(inputMat, resizedMat, cv::Size(imgW_, imgH_));
        uchar* resized_dev;
        CHECK_CUDA(cudaMalloc(&resized_dev, resizedMat.total() * resizedMat.elemSize()));
        CHECK_CUDA(cudaMemcpy(resized_dev, resizedMat.data, resizedMat.total() * resizedMat.elemSize(), cudaMemcpyHostToDevice));
        CHECK_CUDA(cudaMemcpy(imgs_dev + offset, resized_dev, resizedMat.total() * resizedMat.elemSize(), cudaMemcpyDeviceToDevice));
        CHECK_CUDA(cudaFree(resized_dev));
        offset += resizedMat.total() * resizedMat.elemSize();
        index++;
    }
    std::cout << "CameraPerception::setImages : " << images->images.size() << std::endl;
    sampleData_.imgs_dev = imgs_dev;
    std::vector<Box> ego_boxes;
    ego_boxes.clear();
    float time = 0.f;
    bevDetPtrs_->DoInfer(sampleData_, ego_boxes, time);
    RCLCPP_INFO(logger_, "bevDetPtrs : %d", ego_boxes.size());
    cb_(ego_boxes);
    CHECK_CUDA(cudaFree(imgs_dev));
}

bool CameraPerception::fileExists(const std::string& filePath)
{
    std::ifstream file(filePath);
    return file.good();
}

void CameraPerception::sample(YAML::Node& config)
{
    imgN_ = config["N"].as<size_t>();
    imgW_ = config["W"].as<int>();
    imgH_ = config["H"].as<int>();
    isMock_ = config["Mock"].as<bool>();
    modelConfig_ = configPath_ + config["ModelConfig"].as<std::string>();
    imgstageFile_ = configPath_ + config["ImgStageEngine"].as<std::string>();
    bevstageFile_ = configPath_ + config["BEVStageEngine"].as<std::string>();
    std::string camConfigPath = configPath_ + config["CamConfig"].as<std::string>();
    std::cout << " camConfigPath " << std::endl;
    camconfig_ = YAML::LoadFile(camConfigPath);
    sample_ = config["sample"];
    std::vector<std::string> imgs_file;
    std::vector<std::string> imgs_name;
    for (auto file : sample_) {
        imgs_file.push_back(file.second.as<std::string>());
        imgs_name.push_back(file.first.as<std::string>());
    }
    sampleData_.param = camParams(camconfig_, imgN_, imgs_name);
}

void CameraPerception::setBevDetCallback(const onBevDetCallback& cb)
{
    cb_ = cb;
}

void CameraPerception::setBevDetMockCallback(const onBevDetCallback& cb)
{
    mockcb_ = cb;
}

}